#include "camera_driver/mindvision.hpp"

#include <libusb-1.0/libusb.h>

#include <stdexcept>

using namespace std::chrono_literals;
namespace io
{
  MindVision::MindVision(double exposure_ms, double gamma, const std::string &vid_pid, const rclcpp::Logger &logger)
      : CameraBase(logger),
        exposure_ms_(exposure_ms),
        gamma_(gamma),
        handle_(-1),
        vid_(-1),
        pid_(-1)
  {
    set_vid_pid(vid_pid);
    if (libusb_init(NULL))
      RCLCPP_WARN(logger_, "Unable to init libusb!");
  }

  MindVision::~MindVision()
  {
    close();
    libusb_exit(NULL);
    RCLCPP_INFO(logger_, "Mindvision destroyed.");
  }

  void MindVision::read(sensor_msgs::msg::Image &img)
  {
    tSdkFrameHead frame_head;
    BYTE *raw_buffer = nullptr;
    // 设置默认为空图像
    img.width = 0;          // 宽度为0
    img.height = 0;         // 高度为0
    img.data.clear();       // 清空数据缓冲区
    img.step = 0;           // 每行字节数为0
    img.encoding = "";

    // 1. 检查相机句柄是否有效
    if (handle_ == -1)
    {
      throw std::runtime_error("Camera handle invalid (not opened)");
    }

    // 2. 从相机获取一帧图像（超时1000ms）
    int status = CameraGetImageBuffer(handle_, &frame_head, &raw_buffer, 1000);
    if (status != CAMERA_STATUS_SUCCESS)
    {
      throw std::runtime_error("Read frame failed (code: " + std::to_string(status) + ")");
    }

    size_t data_size = frame_head.iWidth * frame_head.iHeight * 3;
    // 预留内存（避免频繁分配）
    if (img.data.capacity() < data_size)
    {
        img.data.reserve(data_size);
    }
    img.data.resize(data_size);  // 确保数据区大小正确

    // 3. 处理图像（转RGB格式）并赋值给输出
    CameraImageProcess(handle_, raw_buffer, img.data.data(), &frame_head);
    img.height = frame_head.iHeight;
    img.width = frame_head.iWidth;
    img.step = frame_head.iWidth * 3;
    img.encoding = "rgb8";
    
    // 4. 释放SDK缓冲区
    CameraReleaseImageBuffer(handle_, raw_buffer);
  }

  bool MindVision::open()
  {
    // 5次重试打开逻辑
    const int MAX_RETRIES = 5;
    for (int i = 0; i < MAX_RETRIES; ++i)
    {
      if (try_open())
      {
        RCLCPP_INFO(logger_, "Mindvision opened");
        return true;
      }

      // 最后一次尝试失败不需要等待
      if (i < MAX_RETRIES - 1)
      {
        RCLCPP_WARN(logger_, "Retrying to open camera... (%d/%d)", i + 2, MAX_RETRIES);
        std::this_thread::sleep_for(1s); // 等待1秒后重试
      }
    }
    return false; 
  }

  bool MindVision::try_open()
  {
    try
    {
      int camera_num = 1;
      tSdkCameraDevInfo camera_info_list;

      // 初始化相机SDK
      CameraSdkInit(1);

      // 枚举相机设备
      CameraEnumerateDevice(&camera_info_list, &camera_num);
      if (camera_num == 0)
      {
        throw std::runtime_error("Not found camera!");
      }

      // 关闭已有的连接
      if (handle_ != -1)
      {
        CameraUnInit(handle_);
        handle_ = -1;
      }

      // 初始化相机，获取句柄
      if (CameraInit(&camera_info_list, -1, -1, &handle_) != CAMERA_STATUS_SUCCESS)
      {
        throw std::runtime_error("Failed to init camera!");
      }

      CameraSetAeState(handle_, FALSE);                                 // 关闭自动曝光
      CameraSetExposureTime(handle_, exposure_ms_ * 1e3);               // 设置曝光(ms转us)
      CameraSetGamma(handle_, static_cast<unsigned int>(gamma_ * 1e2)); // 设置伽马
      CameraSetIspOutFormat(handle_, CAMERA_MEDIA_TYPE_RGB8);           // 设置输出格式为RGB
      CameraSetTriggerMode(handle_, 0);                                 // 设置为连续采集模式
      CameraSetFrameSpeed(handle_, 1);                                  // 设置为低帧率模式
      
      // 开始采集
      CameraPlay(handle_);
      return true;
    }
    catch (const std::exception &e)
    {
      RCLCPP_WARN(logger_, "Error opening camera: %s", e.what());
      reset_usb(); // 重置USB尝试恢复
      return false;
    }
  }
  void MindVision::close()
  {
    if (handle_ != -1)
    {
      CameraUnInit(handle_);
      handle_ = -1; // 置为无效，避免重复关闭
      RCLCPP_INFO(logger_, "Mindvision closed");
    }
  }

  void MindVision::set_vid_pid(const std::string &vid_pid)
  {
    // 查找"vid:pid"格式中的分隔符
    auto index = vid_pid.find(':');
    if (index == std::string::npos)
    {
      RCLCPP_WARN(logger_, "Invalid vid_pid: \"%s\"", vid_pid.c_str());
      return;
    }

    auto vid_str = vid_pid.substr(0, index);  // 提取VID字符串（十六进制）
    auto pid_str = vid_pid.substr(index + 1); // 提取PID字符串（十六进制）

    try
    {
      vid_ = std::stoi(vid_str, 0, 16); // 十六进制转整数（VID）
      pid_ = std::stoi(pid_str, 0, 16); // 十六进制转整数（PID）
    }
    catch (const std::exception &)
    {
      RCLCPP_WARN(logger_, "Invalid vid_pid: \"%s\"", vid_pid.c_str());
    }
  }

  void MindVision::reset_usb() const
  {
    if (vid_ == -1 || pid_ == -1)
      return;

    // https://github.com/ralight/usb-reset/blob/master/usb-reset.c
    // 过 VID 和 PID 在 USB 总线上查找并打开目标设备
    auto handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_);
    if (!handle)
    {
      RCLCPP_WARN(logger_, "Unable to open usb!");
      return;
    }
    // 重置 USB 设备
    if (libusb_reset_device(handle))
      RCLCPP_WARN(logger_, "Unable to reset usb!");
    else
      RCLCPP_INFO(logger_, "Reset usb successfully !");

    libusb_close(handle);
  }

  bool MindVision::set_param(const std::string &param_name, double value)
  {
    // 根据参数名分支处理（仅支持自身的参数）
    if (param_name == "exposure_ms")
    {
      return set_exposure(value);
    }
    else if (param_name == "gamma")
    {
      return set_gamma(value);
    }
    else
    {
      RCLCPP_WARN(logger_, "MindVision does not support param: %s", param_name.c_str());
      return false; // 不支持的参数返回失败
    }
  }

  bool MindVision::set_exposure(double exposure_ms)
  {
    if (exposure_ms < 0.1 || exposure_ms > 1000.0)
    {
      RCLCPP_WARN(logger_, "Exposure out of range (0.1~1000 ms)");
      return false;
    }
    // 迈德威视 SDK：曝光单位是微秒，需转换（ms → us）
    int ret = CameraSetExposureTime(handle_, static_cast<unsigned int>(exposure_ms * 1e3));
    if (ret != CAMERA_STATUS_SUCCESS)
    {
      RCLCPP_ERROR(logger_, "Set exposure failed (code: %d)", ret);
      return false;
    }
    exposure_ms_ = exposure_ms;
    return true;
  }

  bool MindVision::set_gamma(double gamma)
  {
    if (gamma < 0.1 || gamma > 3.0) // 按相机实际范围调整
    {
      RCLCPP_WARN(logger_, "Gamma out of range (0.1~3.0)");
      return false;
    }
    // 迈德威视 SDK：伽马值需乘以 100（如 1.0 → 100）
    int ret = CameraSetGamma(handle_, static_cast<unsigned int>(gamma * 1e2));
    if (ret != CAMERA_STATUS_SUCCESS)
    {
      RCLCPP_ERROR(logger_, "Set gamma failed (code: %d)", ret);
      return false;
    }
    gamma_ = gamma;
    return true;
  }

}
